/*
 * COPYRIGHT (C) 2005-2011
 * RE2, INC.
 * ALL RIGHTS RESERVED
 *
 *
 * THIS WORK CONTAINS VALUABLE CONFIDENTIAL AND PROPRIETARY INFORMATION.
 * DISCLOSURE OR REPRODUCTION WITHOUT THE WRITTEN AUTHORIZATION OF RE2, INC.
 * IS PROHIBITED. THIS UNPUBLISHED WORK BY RE2, INC. IS PROTECTED BY THE LAWS 
 * OF THE UNITED STATES AND OTHER COUNTRIES. IF PUBLICATION OF THE WORK SHOULD 
 * OCCUR, THE FOLLOWING NOTICE SHALL APPLY.
 *
 * "COPYRIGHT (C) 2005-2010 RE2, INC. ALL RIGHTS RESERVED."
 *
 * RE2, INC. DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE, INCLUDING 
 * ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO EVENT SHALL 
 * RE2, INC. BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR 
 * ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER 
 * IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT 
 * OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */


#ifndef POINTCLOUDPROCESSING_H_
#define POINTCLOUDPROCESSING_H_

#include <ros/node_handle.h>
#include <sensor_msgs/PointCloud2.h>
#include "pcl/ModelCoefficients.h"
#include "pcl/point_types.h"
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include "pcl/io/pcd_io.h"
#include "pcl/sample_consensus/method_types.h"
#include "pcl/sample_consensus/model_types.h"
#include "pcl/segmentation/sac_segmentation.h"
#include <geometry_msgs/QuaternionStamped.h>
#include <boost/function.hpp>
#include <boost/thread.hpp>

typedef pcl::PointXYZ PointType;

class PointCloudProcessing{
public:
    PointCloudProcessing(ros::NodeHandle node);
    void init(ros::NodeHandle node);
    void start();
    void stop();

private:

    ros::NodeHandle m_node;
    std::string m_ptCloudTopic;
    bool m_running;
    boost::thread *m_mainThread;

    pcl::PointCloud<PointType> m_origCloud;
    pcl::PointCloud<PointType> m_cloudFiltered;
    pcl::PointCloud<PointType> m_cloudInliers;

    double viewDepthMin;
    double viewDepthMax;
    double viewWidthMin;
    double viewWidthMax;
    double viewHeightMin;
    double viewHeightMax;

    int viewDepthMinInt;

    bool showFilteredPoints;
    bool findCylinderPoints;
    bool findSpherePoints;
    bool findPlanePoints;

    // Subscribers
    ros::Subscriber m_kinectPointCloud2Sub;

    // Publishers
    ros::Publisher m_originalPointsPub;
    ros::Publisher m_filteredPointsPub;
    ros::Publisher m_inlierPointsPub;
    ros::Publisher m_sphereCoefficientsPub;

    void receivePointCloud2(const sensor_msgs::PointCloud2::ConstPtr &msg);
    void filterPoints(pcl::PointCloud<PointType> cloud);

    void flipX(pcl::PointCloud<PointType> *cloud);
    void flipY(pcl::PointCloud<PointType> *cloud);
    void flipZ(pcl::PointCloud<PointType> *cloud);

    void publishPoints();
    void mainLoop();

    void findPlane(pcl::PointCloud<PointType> cloud);
    void findCylinder(pcl::PointCloud<PointType> cloud);
    void findSphere(pcl::PointCloud<PointType> cloud);
};

#endif /* POINTCLOUDPROCESSING_H_ */
